﻿using System;
using System.Collections.Generic;
using System.Collections;
using System.Drawing;
namespace MonoPhysics
{
    public struct Vector
    {
        #region Proprietes
        private float x;
        private float y;
        private float z;

        public float X
        {
            get { return x; }
            set { x = value; }
        }
        public float Y
        {
            get { return y; }
            set { y = value; }
        }
        public float Z
        {
            get { return z; }
            set { z = value; }
        }

        public float DirectionDeg
        { get { return Math.Abs((float)Math.Atan2(y, x)) * 360 / (float)Math.PI; } }

        public float Magnitude
        { get { return (float)Math.Sqrt(x * x + y * y + z * z); } }

        #endregion

        #region Constructeur
        public Vector(float x, float y, float z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }
        public Vector(PointD pt1, PointD pt2)
            : this(pt2.X - pt1.X, pt2.Y - pt1.Y, 0)
        { }
        public Vector(Point pt1, Point pt2)
            : this(pt2.X - pt1.X, pt2.Y - pt1.Y, 0)
        { }
        public Vector(PointD pt)
            : this(pt.X, pt.Y, 0)
        { }
        public Vector(Point pt)
            : this(pt.X, pt.Y, 0)
        { }
        public Vector(Vector v)
            : this(v.x, v.y, v.z)
        { }
        #endregion

        #region Fonctions
        public bool isNull()
        { return (this.x == 0 && this.y == 0 && this.z == 0); }
        public bool isNull(float epsilon, bool inclusive)
        {
            epsilon = Math.Abs(epsilon);
            if (inclusive)
                return (this.x <= epsilon && this.x >= -epsilon && this.y <= epsilon && this.y >= -epsilon && this.z <= epsilon && this.z >= -epsilon);
            else
                return (this.x < epsilon && this.x > -epsilon && this.y < epsilon && this.y > -epsilon && this.z < epsilon && this.z > -epsilon);
        }
        public bool isCollinear(Vector direction)
        { return this.cross(direction).isNull(); }
        public bool isOrthogonal(Vector direction)
        { return this.dot(direction) == 0; }
        //
        //
        public float perp(Vector v)
        { return this.x * v.y - this.y * v.x; }
        public float dot(Vector v)
        { return (this.x * v.x + this.y * v.y + this.z * v.z); }
        public Vector cross(Vector v)
        { return new Vector(this.y * v.z - this.z * v.y, this.z * v.x - this.x * v.z, this.x * v.y - this.y * v.x); }
        public Vector reflexion()
        { return new Vector(-this.x, -this.y, -this.z); }
        public Vector direction()
        {
            float l = this.Magnitude;
            return (l != 0) ? new Vector(this.x / l, this.y / l, this.z / l) : new Vector(0, 0, 0);
        }
        //
        //
        #endregion

        #region Operateurs
        public static Vector operator +(Vector v, Vector v1)
        { return new Vector(v.x + v1.x, v.y + v1.y, v.z + v1.z); }
        public static Vector operator +(Vector v, PointD v1)
        { return new Vector(v.x + v1.X, v.y + v1.Y, v.z); }
        public static Vector operator -(Vector v, PointD v1)
        { return new Vector(v.x - v1.X, v.y - v1.Y, v.z); }
        public static Vector operator -(Vector v, Vector v1)
        { return new Vector(v.x - v1.x, v.y - v1.y, v.z - v1.z); }
        public static Vector operator *(Vector v, Vector v1)
        { return new Vector(v.x * v1.x, v.y * v1.y, v.z * v1.z); }
        public static Vector operator /(Vector v, Vector v1)
        { return new Vector(v.x / v1.x, v.y / v1.y, v.z / v1.z); }

        public static Vector operator *(float v, Vector v1)
        { return new Vector(v * v1.x, v * v1.y, v * v1.z); }
        public static Vector operator *(Vector v1, float v)
        { return new Vector(v * v1.x, v * v1.y, v * v1.z); }
        public static Vector operator /(Vector v1, float v)
        { return new Vector(v1.x / v, v1.y / v, v1.z / v); }

        public static bool operator ==(Vector v, Vector v2)
        { return (v.x == v2.x && v.y == v2.y); }
        public static bool operator !=(Vector v, Vector v2)
        { return (v.x != v2.x || v.x != v2.y); }

        public static explicit operator PointD(Vector v)
        { return new PointD(v.x, v.y); }

        public override string ToString()
        { return "{ X= " + this.x.ToString() + " | Y= " + this.y.ToString() + " | Z= " + this.z.ToString() + " }"; }
        #endregion
    }


    public struct PointD
    {
        private float x;
        private float y;
        public float X
        {
            get { return x; }
            set { x = value; }
        }

        public float Y
        {
            get { return y; }
            set { y = value; }
        }

        public PointD(float x, float y)
        {
            this.x = x;
            this.y = y;
        }

        public PointD(Point pt)
        {
            this.x = pt.X;
            this.y = pt.Y;
        }

        public bool isZero(float tolerence)
        { return (this.x * this.x + this.y * this.y <= tolerence * tolerence); }

        public static PointD operator +(PointD pt, PointD pt2)
        { return new PointD(pt2.x + pt.x, pt2.y + pt.y); }

        public static PointD operator +(PointD p, Vector v)
        { return new PointD(p.x + v.X, p.y + v.Y); }

        public static PointD operator -(PointD p, Vector v)
        { return new PointD(p.x - v.X, p.y - v.Y); }

        public static PointD operator -(PointD pt, PointD pt2)
        { return new PointD(pt.x - pt2.x, pt.y - pt2.y); }

        public static bool operator !=(PointD pt, PointD pt2)
        { return pt.x != pt2.x || pt.y != pt2.y; }


        public static bool operator ==(PointD pt, PointD pt2)
        { return pt.x == pt2.x && pt.y == pt2.y; }

        public static float GetDist(PointD p1, PointD p2)
        {
            float dx = p2.x - p1.x;
            float dy = p2.y - p1.y;
            return (float)Math.Sqrt(dx * dx + dy * dy);
        }

        public override string ToString()
        { return "{ X= " + this.x.ToString() + " | Y= " + this.y.ToString() + " }"; }
    }

    public class SegmentD
    {
        private CircularLinkedListNode<PointD, RigidBody> linkPt1;
        private CircularLinkedListNode<PointD, RigidBody> linkPt2;
        private float orientation = 0;
        public PointD Pt1
        { get { return this.linkPt1.value + linkPt1.innerParent.Pos; } }
        public PointD Pt2
        { get { return this.linkPt2.value + linkPt2.innerParent.Pos; } }

        public CircularLinkedListNode<PointD, RigidBody> LinkPt1
        { get { return this.linkPt1; } }

        public CircularLinkedListNode<PointD, RigidBody> LinkPt2
        { get { return this.linkPt2; } }

        public float Orientation
        {
            get { return orientation; }
            set { this.orientation = value; }
        }

        public float Length
        { get { return PointD.GetDist(Pt1, Pt2); } }

        public SegmentD(CircularLinkedListNode<PointD, RigidBody> pt1, CircularLinkedListNode<PointD, RigidBody> pt2)
        {
            this.linkPt1 = pt1;
            this.linkPt2 = pt2;
        }

        public bool Contains(PointD p)
        {
            PointD p1 = this.Pt1;
            PointD p2 = this.Pt2;
            if (p1.X != p2.X)
            {
                if (p1.X <= p.X && p.X <= p2.X)
                    return true;
                if (p1.X >= p.X && p.X >= p2.X)
                    return true;
            }
            else
            {
                if (p1.Y >= p.Y && p.Y >= p2.Y)
                    return true;
                if (p1.Y <= p.Y && p.Y <= p2.Y)
                    return true;
            }
            return false;
        }

        //public static Lin
    }

    public class SegmentD<T>
    {
        public PointD Pt1;
        public PointD Pt2;
        public T ExtraData;
        public float Length
        { get { return PointD.GetDist(Pt1, Pt2); } }

        public SegmentD(PointD pt1, PointD pt2, T extra)
        {
            this.ExtraData = extra;
            this.Pt1 = pt1;
            this.Pt2 = pt2;
        }

        public bool Contains(PointD p)
        {
            PointD p1 = this.Pt1;
            PointD p2 = this.Pt2;
            if (p1.X != p2.X)
            {
                if (p1.X <= p.X && p.X <= p2.X)
                    return true;
                if (p1.X >= p.X && p.X >= p2.X)
                    return true;
            }
            else
            {
                if (p1.Y >= p.Y && p.Y >= p2.Y)
                    return true;
                if (p1.Y <= p.Y && p.Y <= p2.Y)
                    return true;
            }
            return false;
        }

        public bool ContainsAnypoint(PointD p)
        {
            PointD p1 = this.Pt1;
            PointD p2 = this.Pt2;
            Vector AC = new Vector(p1, p2);
            Vector AB = new Vector(p1, p);
            if (AC.perp(AB) == 0)
            {
                AC = AC.reflexion();
                Vector CB = new Vector(p1, p);
                return (AC.dot(CB) <= 0);
            }
            else
                return false;
        }
        //public static Lin
    }

    public struct SegmentCouple
    {
        private SegmentD s1;
        private SegmentD s2;
        public SegmentCouple(SegmentD s1, SegmentD s2)
        {
            this.s1 = s1;
            this.s2 = s2;
        }

        public bool equals(SegmentCouple s)
        {
            return (((s.s1.LinkPt1 == this.s1.LinkPt1 && s.s1.LinkPt2 == this.s1.LinkPt2) && (s.s2.LinkPt1 == this.s2.LinkPt1 && s.s2.LinkPt2 == this.s2.LinkPt2))
                || ((s.s1.LinkPt1 == this.s2.LinkPt1 && s.s1.LinkPt2 == this.s2.LinkPt2) && (s.s2.LinkPt1 == this.s1.LinkPt1 && s.s2.LinkPt2 == this.s1.LinkPt2)));
        }
    }
}